#include "main.h"
#include "IR.h"
#include "control.h"
#include "string.h"

extern int speedX, speedY, speedZ;
_Bool lineState[5] = {0, 0, 0, 0, 0};
uint8_t state = 0;
Stuff stuff[] = 
{
	{.way = "LRR0", .id = {0x04, 0xBB, 0x32, 0x22, 0xB7, 0x2A, 0x81}},
	{.way = "LRLR0", .id = {0x04, 0xBB, 0x32, 0x22, 0xB7, 0x2A, 0x81}},
};

Stuff currentStuff;

int rSpeed = 50;
int lSpeed = 80;
int forwardSpeed = 300;
int rotateSpeed = 30;
_Bool cornerFlag = 0;
_Bool stopFlag = 0;
char way[10][10];
char currentWay[10] = "RLLR0";
int i = 0;

void irInit(void)	
{
	strcpy(way[0], "LLRR0");
	strcpy(way[1], "LLRR0");
	strcpy(way[2], "LLRR0");
	strcpy(way[3], "LLRR0");
}

void chooseWay(uint8_t n)
{
	strcpy(currentWay, way[n]);
}

void getIR(void)
{
	state = 0;
	
    if(HAL_GPIO_ReadPin(IR1_GPIO_Port, IR1_Pin) == GPIO_PIN_SET)
    {
        state |= 0x10;
    }
    if(HAL_GPIO_ReadPin(IR2_GPIO_Port, IR2_Pin) == GPIO_PIN_SET)
    {
        state |= 0x08;
    }
    if(HAL_GPIO_ReadPin(IR3_GPIO_Port, IR3_Pin) == GPIO_PIN_SET)
    {
        state |= 0x04;
    }
    if(HAL_GPIO_ReadPin(IR4_GPIO_Port, IR4_Pin) == GPIO_PIN_SET)
    {
        state |= 0x02;
    }
    if(HAL_GPIO_ReadPin(IR5_GPIO_Port, IR5_Pin) == GPIO_PIN_SET)
    {
        state |= 0x01;
    }
    
//    lineState[0] = HAL_GPIO_ReadPin(IR1_GPIO_Port, IR1_Pin);
//    lineState[1] = HAL_GPIO_ReadPin(IR2_GPIO_Port, IR2_Pin);
//    lineState[2] = HAL_GPIO_ReadPin(IR3_GPIO_Port, IR3_Pin);
//    lineState[3] = HAL_GPIO_ReadPin(IR4_GPIO_Port, IR4_Pin);
//    lineState[4] = HAL_GPIO_ReadPin(IR5_GPIO_Port, IR5_Pin);
}

void traceLine(void)
{
	switch (state)
	{
		case 0x04://00100
		{
			speedX = 0;
			speedY = forwardSpeed;
			speedZ = 0;
			break;
		}
		case 0x02://00010
		{
			speedX = rSpeed;
			speedY = forwardSpeed;
			speedZ = rotateSpeed;
			break;
		}
		case 0x06://00110
		{
			speedX = rSpeed;
			speedY = forwardSpeed;
			speedZ = rotateSpeed;
			break;
		}
		case 0x01://00001
		{
			speedX = rSpeed;
			speedY = forwardSpeed;
			speedZ = rotateSpeed;
			break;
		}
		case 0x08://01000
		{
			speedX = -lSpeed;
			speedY = forwardSpeed;
			speedZ = rotateSpeed/2;
			break;
		}
		case 0x10://10000
		{
			speedX = -lSpeed;
			speedY = forwardSpeed;
			speedZ = rotateSpeed/2;
			break;
		}
		case 0x12://01100
		{
			speedX = -lSpeed;
			speedY = forwardSpeed;
			speedZ = rotateSpeed/2;
			break;
		}
		case 0x00:
		{
			break;
		}
		default:
		{
			cornerFlag = 1;
			handleTurn();
		}
	}
}

void handleTurn(void)
{
	if(cornerFlag)
	{	
		switch(currentStuff.way[i])
		{
			case '0':
			{
				stopFlag = 1;
				break;
			}
			case 'L':
			{
				HAL_Delay(480);
				speedX = 0;
				speedY = 0;
				speedZ = -300;
				HAL_Delay(1600);
				break;
			}
			case 'R':
			{
				HAL_Delay(750);
				speedX = 0;
				speedY = 0;
				speedZ = 250;
				HAL_Delay(1300);
				break;
			}
			case 'S':
			{
				HAL_Delay(200);
				break;
			}
		}
		cornerFlag = 0;
		i++;
	}
}
